#ifndef PID_CONTROLLER_HPP
#define PID_CONTROLLER_HPP

class PIDController {
public:

    PIDController():kp_(0.01), ki_(0.01), kd_(0.005), prev_error_(0.0), integral_(0.0){

    }

    void resetvalue(double kp, double ki, double kd){
        kp_ = kp;
        ki_ = ki;
        kd_ = kd; 
        prev_error_ = 0.0;
        integral_ = 0.0;
    }

   
    PIDController(double kp, double ki, double kd)
        : kp_(kp), ki_(ki), kd_(kd), prev_error_(0.0), integral_(0.0) {
            decay_factor = 10.0;
        }

    double calculate(double setpoint, double current_value, double dt,int w) {
        double error = (setpoint - current_value) / w;
        error *= decay_factor;
        integral_ += error * dt;
        double derivative = (error - prev_error_) / dt;
        double output = kp_ * error + ki_ * integral_ + kd_ * derivative;
        prev_error_ = error;
        // std::cout << "error:" << error <<std::endl;
        // std::cout << "derivative:" << derivative <<std::endl;
        // std::cout << "integral_:" << integral_ <<std::endl;
        return output;
    }

    void set_decayfactor(double factor){
        decay_factor = factor;
    }

private:
    double kp_, ki_, kd_;
    double prev_error_, integral_;

    double decay_factor;

};

#endif // PID_CONTROLLER_HPP
